#ifndef TASK_CONTROLLER_H
#define TASK_CONTROLLER_H

#include <ros/ros.h>
#include "tools.h"
#include "params.h"
#include "navigation.h"
#include "pcl_match.h"
#include "task_main/DMPose.h"
#define RADIAN_TO_DEGREE 57.324840f


class TaskMain
{
public:
    TaskMain();
    ~TaskMain();

    void init();
    void run();
    void PublishPoseCmd(float x, float y, float a);
    void GetCartographPoseCmdCallback(const geometry_msgs::PoseStamped::ConstPtr& pose);
    void GetTestCmdCallback(const std_msgs::String::ConstPtr& msg);
private:
    ros::NodeHandle nh_;
    ros::Publisher pub_pose_;

    ros::Subscriber carto_sub;
    ros::Subscriber test_sub;

    std::shared_ptr<Tools> tools;
    std::shared_ptr<Navigation> nav_controller;
    std::shared_ptr<PclMatch> pcl_match;
    // 定义params
    std::shared_ptr<Params> params;
    geometry_msgs::PoseStamped g_carto_pose_;
    Point2D g_pose_2d;
};

#endif // TASK_CONTROLLER_H